#!/usr/bin/env python3
# coding=utf-8

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

if __name__ == "__main__":
    rospy.init_node("nav_client") #初始化节点

    ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)#连接的节点和消息包类型

    ac.wait_for_server()#检测movebase是否启动，每启动卡在这里

    goal = MoveBaseGoal()#创建导航消息包
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.pose.position.x = -3.0
    goal.target_pose.pose.position.y = 2.0
    goal.target_pose.pose.position.z = 0.0
    goal.target_pose.pose.orientation.x = 0.0
    goal.target_pose.pose.orientation.y = 0.0
    goal.target_pose.pose.orientation.z = 0.0
    goal.target_pose.pose.orientation.w = 1.0
    ac.send_goal(goal) #发送导航信息
    rospy.loginfo("开始导航...") #发送成功
    ac.wait_for_result()#等待导航结束，没到是会等待

    if ac.get_state() == actionlib.GoalStatus.SUCCEEDED:
        rospy.loginfo("导航成功！")
    else:
        rospy.loginfo("导航失败...")

